/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package com.team1160.breakaway.input;

import com.team1160.breakaway.api.Constants;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStationEnhancedIO;
import edu.wpi.first.wpilibj.DriverStationEnhancedIO.EnhancedIOException;

/**
 *
 * @author CJ
 */
public class InputReader
{
    //TODO GIRL
    //This reads a driverstation
    protected DriverStation driverStation;
    protected DriverStationEnhancedIO enhancedIO;
    protected static InputReader _INSTANCE;

    protected boolean lastKicker;
    protected boolean lastDribbler;


    //---------------------------------------------------------------------

    private InputReader(){
        this.lastDribbler  = false;
        this.lastKicker    = true;
        this.driverStation = DriverStation.getInstance();
        this.enhancedIO    = this.driverStation.getEnhancedIO();
    }

    //---------------------------------------------------------------------

    public static InputReader getInstance(){
        if (_INSTANCE == null)
        {
            _INSTANCE = new InputReader();
        }
        return _INSTANCE;
    }

    //---------------------------------------------------------------------

    public AutoInputState getAutoInputState(){
        AutoInputState autoState = new AutoInputState();
        autoState.autoSwitchOne = this.driverStation.getDigitalIn(Constants.C_DRIVERSTATION_AUTO_SWITCH_ONE);
        autoState.autoSwitchTwo = this.driverStation.getDigitalIn(Constants.C_DRIVERSTATION_AUTO_SWITCH_TWO);
        return autoState;
    }


    //---------------------------------------------------------------------

    public InputState getInputState() throws EnhancedIOException
    {
        //construct new state instance
        InputState currentInputState = new InputState();

        //set joystick values
        readJoysticks(currentInputState);

        //set button/switches
        readButtons(currentInputState);

        //return the state
        return currentInputState;
    }

    //---------------------------------------------------------------------

    protected void readJoysticks(InputState currentInputState)
    {
        currentInputState.drive_xAxis = this.driverStation.getStickAxis(
                Constants.C_DRIVE_JOYSTICK,Constants.JOYSTICK_AXIS_X);

        currentInputState.drive_yAxis = this.driverStation.getStickAxis(
                Constants.C_DRIVE_JOYSTICK, Constants.JOYSTICK_AXIS_Y);
    }

    //---------------------------------------------------------------------

    protected void readButtons(InputState currentInputState)
    {

        //read the dribble switch
        currentInputState.dribbler_on = this.driverStation.getDigitalIn(Constants.C_DRIVERSTATION_DRIBBLER_SWITCH);
        currentInputState.dribbler_speed = -this.driverStation.getAnalogIn(1)/670;

        currentInputState.activate_winch = currentInputState.dribbler_on;
        currentInputState.trigger_claw = currentInputState.dribbler_speed < -0.5;
        
        //read the kicker button (compare to previous value)
        boolean temp = this.driverStation.getDigitalIn(Constants.C_DRIVERSTATION_KICKER_SWITCH);
        if(temp != this.lastKicker)
            currentInputState.toggle_shooter = true;
        else
            currentInputState.toggle_shooter = false;
        this.lastKicker = temp;

        //orient robot via camera
        currentInputState.orient_by_camera = 
                (this.driverStation.getStickButtons(Constants.C_DRIVE_JOYSTICK) &
                                                     Constants.C_ORIENT_BY_CAMERA)
                                == Constants.C_ORIENT_BY_CAMERA;

        //Safety Mode
        currentInputState.safety_mode_enabled =
                (this.driverStation.getStickButtons(Constants.C_DRIVE_JOYSTICK) &
                    Constants.C_SAFETY_MODE_BYTE_VALUE) == Constants.C_SAFETY_MODE_BYTE_VALUE;

        //Stop Buttons
        int temp_buttons = this.driverStation.getStickButtons(Constants.C_DRIVE_JOYSTICK);
        //input
        //11001100000 = 1632
        //1024 512 256 128 64 32 16 8 4 2 1
        if((temp_buttons & 1632) >= 32){
            currentInputState.stop_motors = true;
            byte[] stopped_motors = new byte[4];
            if((temp_buttons & Constants.C_RIGHT_FRONT_MOTOR_BYTE_VALUE) == Constants.C_RIGHT_FRONT_MOTOR_BYTE_VALUE){
                stopped_motors[0] = Constants.ID_DRIVETRAIN_RIGHT_FRONT;
            }else
                stopped_motors[0] = -1;

            if((temp_buttons & Constants.C_RIGHT_BACK_MOTOR_BYTE_VALUE) == Constants.C_RIGHT_BACK_MOTOR_BYTE_VALUE){
                stopped_motors[1] = Constants.ID_DRIVETRAIN_RIGHT_BACK;
            }else
                stopped_motors[1] = -1;

            if((temp_buttons & Constants.C_LEFT_BACK_MOTOR_BYTE_VALUE) == Constants.C_LEFT_BACK_MOTOR_BYTE_VALUE){
                stopped_motors[2] = Constants.ID_DRIVETRAIN_LEFT_BACK;
            }else
                stopped_motors[2] = -1;

            if((temp_buttons & Constants.C_LEFT_FRONT_MOTOR_BYTE_VALUE) == Constants.C_LEFT_FRONT_MOTOR_BYTE_VALUE){
                stopped_motors[3] = Constants.ID_DRIVETRAIN_LEFT_FRONT;
            }else
                stopped_motors[3] = -1;

            currentInputState.stopped_motors = stopped_motors;
        }
    }

    //---------------------------------------------------------------------
}
